#ifndef MAPPOINT_H
#define MAPPOINT_H

#include <iostream>
#include <map>
#include <opencv2/core/core.hpp>
#include <mutex>
#include "Map.h"
#include "KeyFrame.h"
#include "Frame.h"

class Map;
class KeyFrame;
class Frame;


class MapPoint
{
public:
	~MapPoint();

	MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap);

	MapPoint(const cv::Mat &Pos,  Map* pMap, Frame* pFrame, const int &idxF);

	void SetWorldPos(const cv::Mat &Pos);

	cv::Mat GetWorldPos();

	cv::Mat GetNormal();

	KeyFrame* GetReferenceKeyFrame();

	std::map<KeyFrame*,size_t> GetObservations();

	int Observations();

	void AddObservation(KeyFrame* pKF,size_t idx);

	void EraseObservation(KeyFrame* pKF);

	int GetIndexInKeyFrame(KeyFrame* pKF);

	bool IsInKeyFrame(KeyFrame* pKF);

	void SetBadMapPointFlag();

	bool isBadMapPoint();

	void Replace(MapPoint* pMP);

	MapPoint* GetReplaced();

	void IncreaseVisible(int n=1);

	void IncreaseFound(int n=1);

	void ComputeDistinctiveDescriptors();

	void UpdateNormalAndDepth();

	cv::Mat GetDescriptor();

	float GetFoundRatio();

public:
	long unsigned int m_MapPointId;

	long int m_FirstKeyFrameId;

	long int m_FirstFrameId;

	int m_ObsNumber;

	// Variables used by the tracking
	long unsigned int m_TrackReferenceForFrameId;
    long unsigned int m_LastFrameSeenId;
	bool m_TrackInView;
	float m_TrackProjX;
	float m_TrackProjY;
	float m_TrackProjXR;
	float m_TrackViewCos;

	// Variables used by local mapping
	long unsigned int m_BALocalForKeyFrameId;
    long unsigned int m_FuseCandidateForKeyFrameId;

	// Variables used by loop closing
	

	static std::mutex g_GlobalMutex;

protected:
	// Position in absolute coordinates
	cv::Mat m_WorldPos;

	// Keyframes observing the point and associated index in keyframe
	std::map<KeyFrame*,size_t> m_Observations;

	// Mean viewing direction
	cv::Mat m_NormalVector;

	// Best descriptor to fast matching
	cv::Mat m_MapPointDescriptor;

	// Reference KeyFrame
	KeyFrame* m_RefKF;

	// Tracking counters
	int m_VisibleCount;
	int m_FoundCount;

	// Bad flag (we do not currently erase MapPoint from memory)
	bool m_BadMapPoint;
	MapPoint* m_Replaced;

	//Scale invariance distances
	float m_scaleInvDistance;

	Map* m_Map;

	std::mutex m_MutexPos;
	std::mutex m_MutexFeatures;
};


#endif //MAPPOINT_H
